#include <vector>  
#include <opencv2/opencv.hpp> 
#include <cmath>  
#include <chrono>  
#include <thread> // for simulate and exam
using namespace std;  
using namespace cv;  
using namespace std::chrono;  


/*define variable*/
const float GRAVITY =9.81f;
//information for scanned position
vector<float> times; 
vector<Point3f> historicalPositions;  
//infromation for gun
 Point3f gunPosition(0,0,1); 
 float elevationAngle =30.0f; //yangjiao
 float azimuthAngle =0.0f; //pianhangjiao
 float v =12.0f;
 auto startTime = high_resolution_clock::now();  
 float currentTime =0.0f;  
 //steps and timebystep
 int futureSteps =5; 
 float timeStep =1.0f; //second



/*least square to fit line*/
Point2f fitLine(const vector<float>& times, const vector<Point3f>& positions, Point2f& slope, Point2f& intercept) {  
 
 int n = times.size();  
 if (n <2) {  
 return Point2f(0,0); 
 }  
 float sumT =0, sumX =0, sumY =0, sumTX =0, sumTY =0, sumTT =0;
for (int i =0; i < n; ++i) {
sumT += times[i];
sumX += positions[i].x; 
sumY += positions[i].y;
sumTX += times[i] * positions[i].x;
sumTY += times[i] * positions[i].y;
sumTT += times[i] * times[i]; 
}
float slopeX = (n * sumTX - sumT * sumX) / (n * sumTT - sumT * sumT);
float slopeY = (n * sumTY - sumT * sumY) / (n * sumTT - sumT * sumT);
float interceptX = (sumX - slopeX * sumT) / n;
float interceptY = (sumY - slopeY * sumT) / n;

slope = Point2f(slopeX, slopeY); 
intercept = Point2f(interceptX, interceptY); 
}



/*predict future position*/
vector<Point3f> predictPositions(const vector<float>& times, const vector<Point3f>& positions, int futureSteps, float timeStep) {
vector<Point3f> predictedPositions;
int n = times.size();

Point2f slope, intercept;
fitLine(times, positions,slope,intercept);

float lastTime = times.back();
for (int i =1; i <= futureSteps; ++i) {
float futureTime = lastTime + i * timeStep; 
float futureX = slope.x * futureTime + intercept.x; 
float futureY = slope.y * futureTime + intercept.y; 
float futureZ = positions[n-1].z; 
predictedPositions.push_back(Point3f(futureX, futureY, futureZ));
}
return predictedPositions;
}




/*aimpoint*/
Point3f calculateAimPoint(const Point3f& predictedPosition, const Point3f& gunPosition, float elevationAngle, float azimuthAngle, float v) {  

 float elevationRad = elevationAngle * CV_PI /180.0f;  
 //float azimuthRad = azimuthAngle * CV_PI /180.0f;  
 float deltaZ = predictedPosition.z - gunPosition.z; // delta height
 //float horizontalDistance = sqrt(pow(predictedPosition.x - gunPosition.x,2) + pow(predictedPosition.y - gunPosition.y,2)); //horizontal distance
 float requiredDistance = deltaZ / sin(elevationRad);  //direct distance
 float aimX = predictedPosition.x - gunPosition.x;  
 float aimY = predictedPosition.y - gunPosition.y;  
 float aimZ = predictedPosition.z + 0.5 * GRAVITY * pow((requiredDistance * cos(elevationRad) / v),2) - gunPosition.z; // 在目标Z轴高度 

 return Point3f(aimX, aimY, aimZ);  
}  



/*output in main*/
int main() {  

 // simulate for test
 for (int i =0; i <10; ++i) { // 10 steps for previous data
 auto now = high_resolution_clock::now();  
 duration<float> elapsedTime = now - startTime;  
 currentTime = elapsedTime.count();  
 historicalPositions.push_back(Point3f(currentTime *2 ,currentTime *3 +5, currentTime)); 
 times.push_back(currentTime);  
 this_thread::sleep_for(milliseconds(1000));  
 }  


 //output: predict position & aimpoint
 vector<Point3f> predictedPositions = predictPositions(times, historicalPositions, futureSteps, timeStep);  
 cout << "predictPositions:" << endl;  
 for (const auto& pos : predictedPositions) {  
 cout << "(" << pos.x << ", " << pos.y << ", " << pos.z << ")" << endl;  
 }  
 for (const auto& predictedPos : predictedPositions) {  
 Point3f aimPoint = calculateAimPoint(predictedPos, gunPosition, elevationAngle, azimuthAngle, v);  
 cout << "aimPoint: (" << aimPoint.x << ", " << aimPoint.y << ", " << aimPoint.z << ")" << endl;  
 }  

 return 0;  
}